← Back to Main Page Fast Robots | Spring 2026

Lab 6: Orientation Control

Objective

The goal of this lab is to control the robot’s orientation using a PID controller and yaw measurements from the IMU. The controller adjusts the motor speeds to rotate the robot to the target angle and recover from disturbances.

Pre Lab

For this lab, I reused my Lab 5 system architecture, including the BLE communication framework, motor driver interface, and PID controller structure. The main change was replacing the ToF-based distance feedback with IMU-based orientation feedback.

The controller input was modified to use the robot’s yaw angle from the IMU. The yaw is estimated by integrating the gyroscope z-axis over time, after subtracting a calibrated bias: 

const float gz = g_icm.gyrZ() - g_gyro_bias_z_dps;
        g_imu_state.yaw_g += gz * dt;

A key limitation of this approach is drift due to gyro bias. Even a small constant bias causes the integrated yaw to slowly increase over time when the robot is stationary. To reduce this, I calibrated the gyro bias at startup by averaging stationary readings and subtracting this offset from future measurements.

To evaluate the remaining drift, I performed a stationary test for 60 seconds. Over this period, the yaw estimate drifted by approximately 3.38 degrees, corresponding to a drift rate of about 0.056 deg/s. Since this drift is relatively small, I did not implement the DMP.

The gyroscope also has a finite measurement range. The ICM-20948 supports up to ±2000 degrees per second, which is significantly higher than the robot’s actual turning speed, so saturation was not a concern in this application.

Since yaw is bounded between -180° and 180°, I added angle wrapping to ensure the error always represents the shortest rotation direction: 

float err = target_deg - current_deg;
      while (err > 180.0f) err -= 360.0f;
      while (err < -180.0f) err += 360.0f;

The PID controller structure remained similar to Lab 5, but now operates on angular error. The control output is computed and clamped to a normalized range: 

float u = p_term + i_term + d_term;
      u = clamp(u, g_pid.output_min, g_pid.output_max);

In practice, I implemented a PI controller. Since yaw is obtained by integrating gyroscope angular velocity, the derivative of the error effectively represents angular velocity again and is highly sensitive to noise. Including a derivative term did not improve performance and introduced instability, so I set K_d = 0. Although a derivative term with low-pass filtering exists in the code framework, it was not used in the final controller since the derivative gain was set to zero. This avoided amplifying noise from the IMU signal.

To improve stability near the setpoint, I also added a small stop band so the motors turn off when the error is within a few degrees.

Tasks

1. PI Controller, Implementation, and Results

The PI controller computes the control input based on the angular error:

\[ e(t) = \theta_{set} - \theta(t) \]

\[ u(t) = K_p \cdot e(t) + K_i \int e(t)\,dt \]

In discrete form, the controller is implemented as:

\[ u = K_p \cdot e + K_i \cdot \sum e \cdot dt \]

Using the setup described above, I implemented and tuned a PI controller for orientation control. The final gains used were: 

Kp = 0.02  
Ki = 0.0005  
Kd = 0

For the recovery task, I set the robot’s initial orientation as the setpoint and then manually disturbed it by pushing it away. From both the video and the plots, the robot is able to return to its original orientation. The response shows overshoot and oscillation as it corrects itself because of the tape on the wheels, but the error decreases over time and the robot eventually settles near the setpoint.

For the second task, I commanded two sequential 90° turns. The robot first rotated from approximately 0° to 90° and settled near the setpoint, then rotated from 90° to 180° and again converged close to the target. Compared to the recovery task, the response here is smoother and more repeatable, with smaller oscillations after each turn.

Overall, the final PI controller achieved stable and repeatable orientation control. The proportional term provided the main turning response, while the small integral term reduced steady-state error. Although some oscillation remained, the robot was able to complete both required tasks successfully.

2. Wind-up

I reused the wind-up protection from Lab 5 by limiting the growth of the integral term. Without wind-up protection, I observed that the robot would continue turning past the target angle and exhibit larger oscillations before settling. This happens because the integral term keeps accumulating error while the robot is still far from the setpoint, causing the controller to apply excessive correction even after the robot has crossed the target.

With wind-up protection enabled, the response became more stable and the overshoot was reduced. This was especially noticeable during the recovery task, where the robot needed to correct large disturbances. Wind-up protection is also important when testing on different floor surfaces, since changes in traction affect how quickly the robot responds and can otherwise lead to inconsistent behavior.

← Back to Main Page